import ctypes
import logging
import os
import sys
import time

logging.basicConfig(filename='lift.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')

# 调用结构体 POSE
class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')

    def Lift_move(self, height, speed):
        """
        机械臂导轨移动
        :param nsocket:句柄
        :param height: 行程
        :param speed: 速度
        :return:成功返回：0。失败返回:错误码, define.h查询。
        """

        ret = self.pDll.Set_Lift_Height(self.nSocket, height, speed, 1)
        logging.info(f'导轨移动：{ret}')

        return ret

    def Arm_move(self, joint, v):
        """
        机械臂运动
        :param joint: 关节角度
        :param v: 速度
        :return:
        """
        # 机械臂 关节运动
        float_joint = ctypes.c_float * 6
        joint = float_joint(*joint)
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte, ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        ret = self.pDll.Movej_Cmd(self.nSocket, joint, v, 0, 1)
        logging.info(f'机械臂关节运动：{ret}')

        return ret


    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)


def main():
    ROT2 = Arm(ip="192.168.1.18")
    ROT2.Lift_move(230, 20)
    ROT2.Arm_move([0, 0, 0, 0, 0, 0], 20)
    ROT2.Lift_move(434, 20)
    ROT2.Arm_move([-60.042, 0.805, 76.392, 0.001, 100.8, -59.993], 20)

    ROT2.close()


if __name__ == '__main__':
    main()
